// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (Robotics and Perception Group, University of Zurich, Switzerland).
//
// SVO is free software: you can redistribute it and/or modify it under the
// terms of the GNU General Public License as published by the Free Software
// Foundation, either version 3 of the License, or any later version.
//
// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
// details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

#ifndef SVO_POINT_H_
#define SVO_POINT_H_

#include <svo/global.h>
#include <boost/noncopyable.hpp>

namespace g2o {
class VertexSBAPointXYZ;
}
typedef g2o::VertexSBAPointXYZ g2oPoint;

namespace svo {

class Feature;

typedef Eigen::Matrix<double, 2, 3> Matrix23d;

/// A 3D point on the surface of the scene.
class Point : boost::noncopyable {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  enum PointType { TYPE_DELETED, TYPE_CANDIDATE, TYPE_UNKNOWN, TYPE_GOOD };

  static int point_counter_;  //!< Counts the number of created points. Used to
                              //!< set the unique id.
  int id_;                    //!< Unique ID of the point.
  Vector3d pos_;     //!< 3d pos of the point in the world coordinate frame.
  Vector3d normal_;  //!< Surface normal at point.
  Matrix3d
      normal_information_;  //!< Inverse covariance matrix of normal estimation.
  bool normal_set_;  //!< Flag whether the surface normal was estimated or not.
  std::list<Feature*>
      obs_;         //!< References to keyframes which observe the point.
  size_t n_obs_;    //!< Number of obervations: Keyframes AND successful
                    //!< reprojections in intermediate frames.
  g2oPoint* v_pt_;  //!< Temporary pointer to the point-vertex in g2o during
                    //!< bundle adjustment.
  int last_published_ts_;     //!< Timestamp of last publishing.
  int last_projected_kf_id_;  //!< Flag for the reprojection: don't reproject a
                              //!< pt twice.
  PointType type_;            //!< Quality of the point.
  int n_failed_reproj_;  //!< Number of failed reprojections. Used to assess the
                         //!< quality of the point.
  int n_succeeded_reproj_;    //!< Number of succeeded reprojections. Used to
                              //!< assess the quality of the point.
  int last_structure_optim_;  //!< Timestamp of last point optimization

  Point(const Vector3d& pos);
  Point(const Vector3d& pos, Feature* ftr);
  ~Point();

  /// Add a reference to a frame.
  void addFrameRef(Feature* ftr);

  /// Remove reference to a frame.
  bool deleteFrameRef(Frame* frame);

  /// Initialize point normal. The inital estimate will point towards the frame.
  void initNormal();

  /// Check whether mappoint has reference to a frame.
  Feature* findFrameRef(Frame* frame);

  /// Get Frame with similar viewpoint.
  bool getCloseViewObs(const Vector3d& pos, Feature*& obs) const;

  /// Get number of observations.
  inline size_t nRefs() const {
    return obs_.size();
  }

  /// Optimize point position through minimizing the reprojection error.
  void optimize(const size_t n_iter);

  /// Jacobian of point projection on unit plane (focal length = 1) in frame
  /// (f).
  inline static void jacobian_xyz2uv(const Vector3d& p_in_f,
                                     const Matrix3d& R_f_w,
                                     Matrix23d& point_jac) {
    const double z_inv = 1.0 / p_in_f[2];
    const double z_inv_sq = z_inv * z_inv;
    point_jac(0, 0) = z_inv;
    point_jac(0, 1) = 0.0;
    point_jac(0, 2) = -p_in_f[0] * z_inv_sq;
    point_jac(1, 0) = 0.0;
    point_jac(1, 1) = z_inv;
    point_jac(1, 2) = -p_in_f[1] * z_inv_sq;
    point_jac = -point_jac * R_f_w;
  }
};

}  // namespace svo

#endif  // SVO_POINT_H_
